#include <stdint.h>
#include <stdio.h>
#include <string.h>

#include "./SYSTEM/usart/usart.h"
#include "stm32f1xx_hal.h"

#include "bms_type.h"
#include "bms_hal_control.h"

#include "bms_comm.h"
#include "bms_monitor.h"
#include "bms_analysis.h"

#include "freertos.h"
#include "queue.h"

#include "drv_modbus.h"

/* Thread config --------------------------------------------------------------*/
#define COMM_TASK_STACK_SIZE	512
#define COMM_TASK_PRIORITY	  3

/* Private variable -----------------------------------------------------------*/
static TaskHandle_t CommTaskHandler = NULL;
static uint8_t prvBuffer[256] = {0};

/* Modbus Data ----------------------------------------------------------------*/
BMS_ModbusRegister_0x03 modbus0x03Register = {0};
BMS_ModbusRegister_0x02 modbus0x02Register = {0};

/* Private function -----------------------------------------------------------*/
static void BMS_CommTaskEntry( void *paramter );
static void BMS_CommUpdate0x03RegisterData();


void BMS_CommInit( void )
{
	bool ret = true;
	
	// Init modbus
	ModbusRTU_InitTypedef initStructTypedef;
	initStructTypedef.idleUs = 50;
	ret = ModbusRTU_Init(&initStructTypedef);
	
	if( !ret )
	{
		printf("Init modbus fail\n");
	}
		
	BaseType_t res = xTaskCreate( BMS_CommTaskEntry,
															  "protect",
	                              COMM_TASK_STACK_SIZE,
	                              NULL,
															  COMM_TASK_PRIORITY,
															  &CommTaskHandler );
	
	if(res != pdPASS)
	{
		printf("Create Comm Task Fail\r\n");
	}
}

static void BMS_CommTaskEntry( void *paramter )
{
	while(1)
	{
		ModbusRTU_EventLoopStart();
	}
}

/**
  * @brief Update 0x03 register Data from @BMS_MonitorData and @BMS_AnalySisData
  *
  */
static void BMS_CommUpdate0x03RegisterData()
{
	int i = 0;
	
	// Fill with AnalysisData
	modbus0x03Register.SOC               = (uint16_t)(BMS_AnalysisData.SOC * 1000);
	modbus0x03Register.RealCapaity       = (uint16_t)( BMS_AnalysisData.CapacityReal * 1000 ); 
	modbus0x03Register.RemainCapacity    = (uint16_t)( BMS_AnalysisData.CapacityRemain * 1000 );
	modbus0x03Register.CellMaxVoltage    = (uint16_t)( BMS_AnalysisData.CellVoltMax * 1000 );
	modbus0x03Register.CellMinVoltage    = (uint16_t)( BMS_AnalysisData.CellVoltMin * 1000 );
	modbus0x03Register.DifferenceVoltage = (uint16_t)( BMS_AnalysisData.MaxVoltageDifference * 1000 );
	modbus0x03Register.AverageVoltage    = (uint16_t)( BMS_AnalysisData.AverageVoltage * 1000 );
	modbus0x03Register.RealPower         = (int16_t)( BMS_AnalysisData.PowerReal * 1000 );

	
	                                       
	// Fill with MonitorData
	modbus0x03Register.PackVoltage       = (uint16_t)( BMS_MonitorData.BatteryVoltage * 1000 );
	modbus0x03Register.Current           = (int16_t)( BMS_MonitorData.BatteryCurrent * 1000);
	modbus0x03Register.Temperature       = (int16_t)( BMS_MonitorData.CellTemp[0] * 10 );
	for( i = 0; i < BMS_CELL_MAX; ++i )
	{
		modbus0x03Register.cellVoltage[i] = BMS_MonitorData.CellVoltage[i] * 1000;
	}
	
	modbus0x03Register.ChargeMosStatus = BMS_MonitorData.isChMosOpen;
	modbus0x03Register.DischargeMosStatus = BMS_MonitorData.isDisMosOpen;
}

/**
  * @brief Implement the function modBus lib need. @See file drv_modbus_port.c
  *
  */
void ModbusRTU_0x03_HandlerPort( uint8_t * buffer, uint8_t dataLen )
{
	int i = 0;
	int cnt = 0;
	int counter = 0;
	uint16_t crcCode = 0;
	int maxRegisterCnt = sizeof( BMS_ModbusRegister_0x03 ) / sizeof( uint16_t );
	
	BMS_CommUpdate0x03RegisterData();
		
	uint16_t addr = buffer[2];
	addr = ( addr << 8 ) | buffer[3];
	if( addr >  maxRegisterCnt )
	{
		return;
	}
	
	cnt = buffer[4];
	cnt = ( cnt << 8 ) | buffer[5];
	
	if( addr + cnt > maxRegisterCnt )
	{
		return;
	}
	
	uint16_t *pointer = ((uint16_t *)&modbus0x03Register) + addr;
	memset(prvBuffer, 0, sizeof(prvBuffer));
	
	// Fill data frame back
	prvBuffer[counter++] = buffer[0];
	prvBuffer[counter++] = 0x03;
	prvBuffer[counter++] = 2 * cnt;
	
	for( i = 0; i < cnt; ++i)
	{
		prvBuffer[counter++] = (pointer[i] >> 8);
		prvBuffer[counter++] = (pointer[i] & 0xff);
	}
	
	crcCode = ModbusRTU_CRC16(prvBuffer, counter);
	prvBuffer[counter++] = crcCode & 0xff;
	prvBuffer[counter++] = ( crcCode >> 8 ) & 0xff;
	
	for( i = 0; i < counter; ++i )
	{
		while (!(USART1->SR & USART_SR_TXE)); 
    USART1->DR = (prvBuffer[i] & 0xFF);   
	}
}

void ModbusRTU_0x06_HandlerPort( uint8_t *buffer, uint8_t dataLen )
{
	uint16_t startAddress = 0;
	uint16_t data = 0;
	startAddress = buffer[2];
	startAddress = ( startAddress << 8 ) | buffer[3];
	data = buffer[4];
	data = ( data << 8 ) | buffer[5];
	
	if( startAddress == 16)
	{
		if( data != 0 )
		{
			BMS_HalCtrlCharge( BMS_STATE_ENABLE );
		}
		else
		{
			BMS_HalCtrlCharge( BMS_STATE_DISABLE );
		}
	}
	
	else if ( startAddress == 17 )
	{
		if( data != 0 )
		{
			BMS_HalCtrlDischarge( BMS_STATE_ENABLE );
		}
		else
		{
			BMS_HalCtrlDischarge( BMS_STATE_DISABLE );
		}
	}
}


